/*        

该任务用于接收来自各个不同的控制方式所期望的控制指令 将其集中统一化后分发给各个模块

*/			
					
		





#ifndef _CMD_H
#define _CMD_H
#include "struct_typedef.h"
#include "device.h"


#define ODOM (0x04)
#define PICK (0x06)

#define SICK_CAIL (0x07)
#define RCTRL (0x08)
#define MID (0x09)

typedef enum{
	 RC,
	 CAMERA_PICK,//取球
   SICK_CAILING,
	 MID_NAVI,
 }CMD_Chassis_CtrlType_e;

typedef  enum{
	 RELAXED,//异常模式
	 NORMAL,
	 GYRO_STAY,
	 PUT
  }CMD_Chassis_mode_e;
/*该结构体负责接收和解析地盘模块所需要的控制指令*/
typedef struct{

 CMD_Chassis_CtrlType_e type;

 CMD_Chassis_mode_e mode;


}CMD_Chassis_Ctrl_t;



/* 拨杆位置 */
typedef enum {
  CMD_SW_ERR = 0,
  CMD_SW_UP = 1,
  CMD_SW_MID = 3,
  CMD_SW_DOWN = 2,
} CMD_SwitchPos_t;

typedef struct {
  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */

  float ch_res; /* 第五通道值 */

  CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */
  CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */


  uint16_t key; /* 按键值 */

  uint16_t res; /* 保留，未启用 */

} __attribute__((packed))CMD_RC_t;

typedef	struct
	{
		fp32 Vx;
		fp32 Vy;
		fp32 Vw;
	}OpsHopeVector_t;
	
typedef struct {
	
  OpsHopeVector_t out;
	
} CMD_ACTION_t;

typedef struct {
 uint8_t  status_fromnuc;
 uint8_t  ctrl_status;  //取其中每一个二进制位用作通信
  struct 
  {
  fp32 vx;
  fp32 vy;
  fp32 wz;
  }navi;
 struct 
 {
	 fp32 posx;
   fp32 posy;
   fp32 posw;
 }pick;
  struct
	{
		fp32 angle;
	}sick_cali;
} CMD_NUC_t;

typedef struct{
	 fp32 posy;
   fp32 posx;
   fp32 posw;	
}CMD_FOR_PICK;


typedef  struct {
  fp32 vx;
  fp32 vy;
  fp32 wz;
}CMD_FOR_NAVI;
typedef struct {
   uint8_t cmd_status;//由nuc判断并决定
   uint8_t raw_status;
	 
	 uint8_t status[7];
	//00001  0x01 不用减速锁中间球直取 
	//01001 0x09   减速锁中间球直取
	//10011 0x13  不锁不减速 取角球 
	//11011 0x1B   不锁减速 取角球
	//10101 0x     不锁用陀螺仪锁角速度 判断左右半场
	/*
	 uint8_t pit_status; //相机控制帧 1//找球
	
	 uint8_t ball_pos  //球的位置帧  0//中心  1//角球
	
	 uint8_t isrigiht  //  左右半场  
	
	 uint8_t dis_towall  //0是不用减  1是需要减速
	 
	 uint8_t chassis_mode // 0是锁姿态 1是不锁
	
	 uint8_t navi_slow //1是减速 0 是不减速
	 
	 uint8_t avoid_ball //1是开检测避障  0是不开
	
	*/
	
   fp32 Vx;
   fp32 Vy;
   fp32 Vw;

   fp32 poscamear;
	  
   fp32 key_ctrl;
	 
	 fp32 forsick_wz;
	
	CMD_Chassis_Ctrl_t C_cmd;
	CMD_FOR_NAVI C_navi;
	CMD_FOR_PICK C_pick;
	
} CMD_t;


int8_t CMD_Init(CMD_t *cmd);

int8_t CMD_ParseRc(CMD_t *cmd,CMD_RC_t *rc);

int8_t CMD_ParseAct(CMD_t *cmd,CMD_ACTION_t *act);

int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n);


int8_t CMD_SwitchStatus(CMD_t *cmd);

int8_t CMD_CtrlSet(CMD_t *cmd);

#endif 

